# 本程序读取707的原始数据，将他们进行处理，保存在707_deal.txt文件中
# 主要需要进行处理的数据包括：
# 序号        名称                 长度
# 1.          文头                 1
# 2.         工作状态               1
# 3.        数据长度                2
# 4.        长度校验                1
# 5.        工作时间                4
# 6.        状态时间                4
# 7.        工作状态                1
# 8.        故障字                  1
# 9.        X轴角速                 4
# 10.       Y轴角速                 4
# 11.       Z轴角速                 4
# 12.       卫星导航                12
# 13.       X                      4
# 14.       Y                      4
# 15.       Z                      4
# 16.        带号                   1
# 17.        里程                   4
# 18.        路程                   4
# 19.        东北高速度              12
# 20.        倾斜角                 4
# 21.        俯仰角                 4
# 22.        方位角                 4
# 23.        坐标北                 4
# 24.        XYZ加速度              12
import time
import csv
__work_time_ = 2
lasttime = 8904.835000000001
__state_name_ = 14
__X_theta_rate_ = 6
__Y_theta_rate_ = __X_theta_rate_ + 4
__Z_theta_rate_ = __X_theta_rate_ + 8   # 24
__X_position_ = __Z_theta_rate_ + 4
__Y_position_ = __X_position_ + 4

__East_Velocity_ = __Y_position_ + 4
__North_Velocity_ = __East_Velocity_ + 4
__Up_Velocity_ = __East_Velocity_ + 8
__Roll_ = __Up_Velocity_ + 4
__Pitch_ = __Roll_ + 4
__Raw_ = __Roll_ + 8
__coder_ = __Raw_ + 4
__coder_2 = __coder_ + 1
__Z_position_ = __coder_2 + 1


def bytes_4_to_byte_32(data_4byte):
    time1 = data_4byte[3]
    time2 = data_4byte[2]
    time3 = data_4byte[1]
    time4 = data_4byte[0]
    return time1 + time2 + time3 + time4
def change_16_to_10(data_16):
    data_10 = int(data_16, 16)
    return data_10

def unsigned_data_deal_1e6(data):
    return change_16_to_10(bytes_4_to_byte_32(data)) * 1e-6 

def signed_data_deal_1e6(data):
    data_byte_32 = bytes_4_to_byte_32(data)
    data_int_10 = change_16_to_10(data_byte_32)
    data_yuan = get_acc(data_int_10)
    return data_yuan * 1e-6
def get_acc_8(data_buma):
    if data_buma & 0x80 == 0x80:
        data_yuanma = (~data_buma + 1) & 0xff
        data_yuanma = -data_yuanma

    else:
        data_yuanma = data_buma

    return data_yuanma

def get_acc(data_buma):
    if data_buma & 0x80000000 == 0x80000000:
        data_yuanma = (~data_buma + 1) & 0xffffffff
        data_yuanma = -data_yuanma

    else:
        data_yuanma = data_buma

    return data_yuanma
lasttime = 8904.835000000001
error_data_num = 0
def deal_data(read_data):
    dealed_data = []
    global lasttime
    global error_data_num
    if read_data == '':
        return 0
    else:
        read_data = read_data.split(' ')
        # 工作时间 0
        print(read_data[6:10])
        work_time = bytes_4_to_byte_32(read_data[6:6 + 4])
        work_time_int = change_16_to_10(work_time) * 5e-3
        print(lasttime)
        print(work_time_int)
        dealed_data.append(work_time_int)
        if((work_time_int - lasttime) > 0.011):
            print("data error")
            #dealed_data.append('aaaaaaaa11111111111111####################')
            error_data_num+=1
        lasttime = work_time_int
        
        
        #状态名字 2
        if read_data[__state_name_] == '00':
            state_name = 0
        elif read_data[__state_name_] == '20' or read_data[__state_name_] == '30' or read_data[__state_name_] == '60':
            state_name = 1
        else:
            state_name = 2
        dealed_data.append(state_name)
    #     # xyz角速度 3 4 5
    #     w_x = signed_data_deal_1e6(read_data[__X_theta_rate_: __X_theta_rate_ + 4])
    #     dealed_data.append(w_x)
    #     w_y = signed_data_deal_1e6(read_data[__Y_theta_rate_: __Y_theta_rate_ + 4])
    #     dealed_data.append(w_y)
    #     w_z = signed_data_deal_1e6(read_data[__Z_theta_rate_: __Z_theta_rate_ + 4])
    #     dealed_data.append(w_z)
    #     # 姿态信息 9 10 11
    #     pitch = 1e4 * signed_data_deal_1e6(read_data[__Pitch_: __Pitch_ + 4])
    #     dealed_data.append(pitch)
    #     roll = 1e4 * signed_data_deal_1e6(read_data[__Roll_: __Roll_ + 4])
    #     dealed_data.append(roll)
    #     row = 1e4 * unsigned_data_deal_1e6(read_data[__Raw_: __Raw_ + 4])
    #     dealed_data.append(row)
    #     # 东北高速度V 12 13 14
    #     east_v = signed_data_deal_1e6(read_data[__East_Velocity_: __East_Velocity_ + 4]) * 1e4
    #     dealed_data.append(east_v)
    #     north_v = signed_data_deal_1e6(read_data[__North_Velocity_: __North_Velocity_ + 4]) * 1e4
    #     dealed_data.append(north_v)
    #     # if north_v > 0:
    #     # print('北向速度%d', north_v)
    #     up_v = signed_data_deal_1e6(read_data[__Up_Velocity_: __Up_Velocity_ + 4]) * 1e4
    #     dealed_data.append(up_v)
    #     # xyz位移 15 16 17
    #     x_move = 1e5 * signed_data_deal_1e6(read_data[__X_position_: __X_position_ + 4])
    #     dealed_data.append(x_move)
    #     y_move = 1e5 * signed_data_deal_1e6(read_data[__Y_position_: __Y_position_ + 4])
    #     dealed_data.append(y_move)
    #     z_move = 1e5*signed_data_deal_1e6(read_data[__Z_position_: __Z_position_ + 4])
    #     dealed_data.append(z_move)
    #    # 编码器数据
    #     coder_data = read_data[__coder_:__coder_+1]
        
    #     coder_data = change_16_to_10(coder_data[0])
    #     coder_data = get_acc_8(coder_data)
    #     dealed_data.append(coder_data)
    #     coder_data1 = read_data[__coder_2:__coder_2+1]
        
    #     coder_data1 = change_16_to_10(coder_data1[0])
    #     coder_data1 = get_acc_8(coder_data1)
    #     dealed_data.append(coder_data1)
        return dealed_data

def write_static_north(data, file):
    str1 = ''
    # for i in range(len(data)):
    #     str1 = str1 + str(data[i]) + ' '
    str1 = str(data[0])
    str1 = str1 + '\n'
    file.write(str1)


def navigation_data(data, file):
    str1 = ''
    # for i in range(len(data)):
    #     str1 = str1 + str(data[i]) + ' '
    str1 = str(data[0])
    str1 = str1 + '\n'
    file.write(str1)
#####################################################
files = open('./12_18/Jetson_save_12_18_18_57_.txt', 'r')
file_static = open('./12_18/static_12_18_18_57_.csv', 'w')
file_navi = open('./12_18/navigation_12_18_18_57__.csv', 'w')

while True:
    #time.sleep(0.5)
    error_data_time = []
    data = files.read(309)
    dealed_data = deal_data(data)
    if dealed_data == 0:
        files.close()
        file_static.close()
        file_navi.close()
        print("close files")
        break
    elif(dealed_data == -1 ):
        print("程序处理出现问题")
        break
    else:
        if dealed_data[1] == 1:
            write_static_north(dealed_data, file_static)
        elif dealed_data[1] == 2:
            navigation_data(dealed_data, file_navi)
        else:
            pass

print(error_data_num)

